#include <fstream>
#include <iostream>

#include <osg/Point>
#include <osg/Group>
#include <osgDB/ReadFile>
#include <osgViewer/ViewerEventHandlers>
#include <osgViewer/Viewer>

#include <pcl/io/pcd_io.h>


const char* vertCode = {
    "uniform sampler2D defaultTex;\n"
    "uniform int width;\n"
    "uniform int height;\n"
    "varying float brightness;\n"
    "void main()\n"
    "{\n"
    "    float r = float(gl_InstanceID) / float(width);\n"
    "    vec2 uv = vec2(fract(r), floor(r) / float(height));\n"
    "    vec4 texValue = texture2D(defaultTex, uv);\n"
    "    vec4 pos = gl_Vertex + vec4(texValue.xyz, 1.0);\n"
    "    brightness = texValue.a;\n"
    "    gl_Position = gl_ModelViewProjectionMatrix * pos;\n"
    "}\n"
};

const char* fragCode = {
    "varying float brightness;\n"
    "void main()\n"
    "{\n"
    "    gl_FragColor = vec4(brightness, brightness, brightness, 1.0);\n"
    "}\n"
};

osg::Geometry* createInstancedGeometry(osg::Image* img, unsigned int numInstances)
{
    osg::ref_ptr<osg::Geometry> geom = new osg::Geometry;
    geom->setUseDisplayList(false);
    geom->setUseVertexBufferObjects(true);
    geom->setVertexArray(new osg::Vec3Array(1));
    geom->addPrimitiveSet(new osg::DrawArrays(GL_POINTS, 0, 1, numInstances));

    osg::ref_ptr<osg::Texture2D> texture = new osg::Texture2D;
    texture->setImage(img);
    texture->setInternalFormat(GL_RGBA32F_ARB);
    texture->setFilter(osg::Texture2D::MIN_FILTER, osg::Texture2D::LINEAR);
    texture->setFilter(osg::Texture2D::MAG_FILTER, osg::Texture2D::LINEAR);
    geom->getOrCreateStateSet()->setTextureAttributeAndModes(0, texture.get());
    geom->getOrCreateStateSet()->addUniform(new osg::Uniform("defaultTex", 0));
    geom->getOrCreateStateSet()->addUniform(new osg::Uniform("width", (int)img->s()));
    geom->getOrCreateStateSet()->addUniform(new osg::Uniform("height", (int)img->t()));

    osg::ref_ptr<osg::Program> program = new osg::Program;
    program->addShader(new osg::Shader(osg::Shader::VERTEX, vertCode));
    program->addShader(new osg::Shader(osg::Shader::FRAGMENT, fragCode));
    geom->getOrCreateStateSet()->setAttributeAndModes(program.get());
    return geom.release();
}

osg::Geometry* readPointDataTxt(const std::string& file, unsigned int w, unsigned int h)
{
    std::ifstream is(file.c_str());
    if (!is) return NULL;

    osg::ref_ptr<osg::Image> image = new osg::Image;
    image->allocateImage(w, h, 1, GL_RGBA, GL_FLOAT);

    unsigned int density, brightness;
    osg::BoundingBox boundBox;
    float* data = (float*)image->data();
    while (!is.eof())
    {
        osg::Vec3 pos;
        is >> pos[0] >> pos[1] >> pos[2] /*>> density >> brightness*/;
        boundBox.expandBy(pos);

        *(data++) = pos[0];
        *(data++) = pos[1];
        *(data++) = pos[2];
        *(data++) = 126.0 / 255.0;
    }

    osg::ref_ptr<osg::Geometry> geom = createInstancedGeometry(image.get(), w * h);
    geom->setInitialBound(boundBox);
    geom->getOrCreateStateSet()->setAttributeAndModes(new osg::Point(5.0f));
    return geom.release();
}

osg::Geometry* readPointDataPcd(const std::string& file, unsigned int w, unsigned int h)
{

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile(file, *cloud);

    osg::ref_ptr<osg::Image> image = new osg::Image;
    image->allocateImage(w, h, 1, GL_RGBA, GL_FLOAT);

    unsigned int density, brightness;
    osg::BoundingBox boundBox;
    float* data = (float*)image->data();

    for (auto i = 0; i < cloud->size(); i++)
    {
        osg::Vec3d pos(cloud->points[i].x, cloud->points[i].z, cloud->points[i].y);
        boundBox.expandBy(pos);
        *(data++) = pos[0];
        *(data++) = pos[1];
        *(data++) = pos[2];
        *(data++) = 255.0 / 255.0;
    }

    osg::ref_ptr<osg::Geometry> geom = createInstancedGeometry(image.get(), w * h);
    geom->setInitialBound(boundBox);
    geom->getOrCreateStateSet()->setAttributeAndModes(new osg::Point(0.5f));
    return geom.release();
}

int main()
{
    osg::ref_ptr<osg::Geode> geode = new osg::Geode;


    //geode->addDrawable(readPointData("E:\\Code\\fish-point-cloud-library\\Project_PointCloudLibrary\\Project_20231116\\cloud.txt", 512, 512));
    
    geode->addDrawable(readPointDataPCD("C:\\Users\\asus\\Desktop\\data\\1.pcd", 512, 512));
    //geode->addDrawable(readPointData("E:\\Code\\osg - viewer\\ProjectOSGViewer\\OSGViewerSARibbon\\data\\cloud_txt\\cloud.txt", 512, 512));
    osg::ref_ptr<osg::Group> root = new osg::Group;
    root->addChild(geode.get());

    osgViewer::Viewer viewer;
    viewer.setSceneData(root.get());
    viewer.addEventHandler(new osgViewer::StatsHandler);
    viewer.addEventHandler(new osgViewer::WindowSizeHandler);
    return viewer.run();
}

